//quad_terimal一个基于Wio Terminal作为硬件载体，用于控制四足机器人的开源操控终端项目
//版本维护：Tinymal RC机器人世界  QQ群607339413

#include <rpcWiFi.h>
#include <WiFiUdp.h>
#include <TFT_eSPI.h>
#include "LIS3DHTR.h"
#include "ocu.h"
#include "Seeed_FS.h" // Including SD card library
#include "RawImage.h"  // Including image processing library
#include "seeed_line_chart.h" // include the line chart library
#include "SD/Seeed_SD.h"
#include "stdio.h"
#include "Free_Fonts.h" // Include the header file attached to this sketch

#define FAST_TEST 0 //上电跳过Wifi自动搜索
_NAV_RX nav_rx;
_OCU ocu;
LIS3DHTR<TwoWire> lis;
 
File myFile; 
int rc_connect=0;
int robot_connect = 0;
int robot_loss_cnt = 0;
int win_sel=1;//默认界面显示1->主界面  0->上帝视角
int wifi_search=0;

String networkName_set;
#if 0//自动默认连接的Wifi
  char *ssid = "Corgi-M1-001";
  char *password = "goodlife";
  char *udpAddress = "192.168.1.168";
  int udpPort = 8889;
#else
  char *ssid = "Corgi12-M1";
  char *password = "goodlife";
  char *udpAddress = "192.168.1.120";
  int udpPort = 8889;
#endif

int robot_sel=0;
char *ssid_sel[10];
char *password_sel[10];
char *udpAddress_sel[10];
int udpPort_sel[10];

boolean connected = false;//robot 连接标志位

WiFiUDP Udp;
//=================================TF控件初始化====================================================
TFT_eSPI tft = TFT_eSPI();

TFT_eSprite graph1 = TFT_eSprite(&tft);  // Sprite object graph1
TFT_eSprite stext2 = TFT_eSprite(&tft);  // 通知

TFT_eSprite content_att[6] = { TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft),
TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft) };   
TFT_eSprite txt_att[6] = { TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft), 
TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft) };       

TFT_eSprite content_mode[4] = { TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft) };   
TFT_eSprite txt_mode[4] = { TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft)};      

TFT_eSprite content_buttion[12] = { 
TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft), 
TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft),
TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft), 
TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft)};   

TFT_eSprite content_q = TFT_eSprite(&tft);   

TFT_eSprite txt_q[12] = {
  TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft),
  TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft),
  TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft),
  TFT_eSprite(&tft), TFT_eSprite(&tft), TFT_eSprite(&tft)
};   

//-----------曲线配置
int graphVal = 1;
int delta = 1;
int grid[3] = {0};
int tcount = 0;

int q_now[12] = { 0 };
int Line_Height = 16;  //字符的高度

int buttonState[3] = { 0 };      // variable for reading the pushbutton status
int WaytbuttonState[5] = { 0 };  // variable for reading the pushbutton status

int window_sel = 0;
int ocu_mode =BUTTON_RC; //0->配置 1->操控
int G_W = 305;
int G_H = 120;

int notice = 0;
int button_mode = BUTTON_RC;

int packetSize_udp = 0;

 
void taskOne(void *parameter)//UDP线程
{
  static float timer=0;
  while(1)
  {
    float dt=Get_Cycle_T(1);
    if(connected){
      timer+=dt;
      Udp.beginPacket(udpAddress,udpPort);
      if(timer>0.1){
        timer=0;
        send_udp1();
      }
      else
        send_udp2();

      for (int i=0; i<usb_send_cnt; i++){
        Udp.write(SendBuff_USB[i]);   // the message to send
      } 
      Udp.endPacket();

      int packetSize = Udp.parsePacket(); //获取当前队首数据包长度
      packetSize_udp=packetSize;
      if (packetSize)                     //如果有数据可用
      {
          char buf[packetSize];
          Udp.read(buf, packetSize); //读取当前包数据
          UDP_RX_PROCESS(buf,packetSize);
      }
      usb_send_cnt=0;
    }
    vTaskDelay(5 / portTICK_PERIOD_MS);//延时300ms
  }
  vTaskDelete(NULL);
}

float rc[2]={0,0};
int bat=0;
int key_sel[4]={0};
int key_o[5]={0};
int connect_io=0;
float loss_cnt_io=0;
void Data_Receive_Anl(char *data_buf,char num)
{
	char sum = 0;
	char i;
	for( i=0;i<(num-1);i++)
		sum += *(data_buf+i);
	if(!(sum==*(data_buf+num-1)))		return;		//判断sum
	if(!(*(data_buf)==0xAA && *(data_buf+1)==0xAF))		return;		//判断帧头
	if(*(data_buf+2)==0x01)//ALL
  {
    connect_io=1;
    loss_cnt_io=0;
    //Serial.println("good");
    rc[0]=(float)((int16_t)(*(data_buf+4)<<8)|*(data_buf+5));
    rc[1]=(float)((int16_t)(*(data_buf+6)<<8)|*(data_buf+7));  
    bat=(float)((int16_t)(*(data_buf+8)<<8)|*(data_buf+9));  

    rc[0]=LIMIT((float)(rc[0]-1500)/500.,-1,1);
    rc[1]=LIMIT((float)(rc[1]-1500)/500.,-1,1);
    
    key_sel[0]=*(data_buf+10);
    key_sel[1]=*(data_buf+11);
    key_sel[2]=*(data_buf+12);
    key_sel[3]=*(data_buf+13);

    key_o[0]=*(data_buf+14);
    key_o[1]=*(data_buf+15);
    key_o[2]=*(data_buf+16);
    key_o[3]=*(data_buf+17);
    key_o[4]=*(data_buf+18);

//    Serial.print(rc[0]*100);
//    Serial.print(" ");
//    Serial.print(rc[1]*100);
//    Serial.print(" ");
//    Serial.println(bat);
//    
//   Serial.print(key_sel[0]);
//   Serial.print(" ");
//   Serial.print(key_sel[1]);
//   Serial.print(" ");
//   Serial.print(key_sel[2]);
//   Serial.print(" ");
//   Serial.print(key_sel[3]);
//   Serial.print(" ");
//   Serial.print(key_o[0]);
//   Serial.print(" ");
//   Serial.print(key_o[1]);
//   Serial.print(" ");
//   Serial.print(key_o[2]);
//   Serial.print(" ");
//   Serial.print(key_o[3]);
//   Serial.print(" ");
//   Serial.println(key_o[4]);
  }
}
 
char RxBuffer[50];
char RxState = 0;
char RxBufferNum = 0;
char RxBufferCnt = 0;
char RxLen = 0;
char com_data ;
static char _data_len = 0,_data_cnt = 0;
void USART_IRQHandler(char com_data)
{ 
		if(RxState==0&&com_data==0xAA)
		{
			RxState=1;
			RxBuffer[0]=com_data;
		}
		else if(RxState==1&&com_data==0xAF)
		{
			RxState=2;
			RxBuffer[1]=com_data;
		}
		else if(RxState==2&&com_data>0&&com_data<0XF1)
		{
			RxState=3;
			RxBuffer[2]=com_data;
		}
		else if(RxState==3&&com_data<50)//MAX_send num==50
		{
			RxState = 4;
			RxBuffer[3]=com_data;
			_data_len = com_data;
			_data_cnt = 0;
		}
		else if(RxState==4&&_data_len>0)
		{
			_data_len--;
			RxBuffer[4+_data_cnt++]=com_data;
			if(_data_len==0)
				RxState = 5;
		}
		else if(RxState==5)
		{
			RxState = 0;
			RxBuffer[4+_data_cnt]=com_data;
			Data_Receive_Anl(RxBuffer,_data_cnt+5);
		}
		else
			RxState = 0;
}

void taskTwo(void *parameter)//Uart线程  有时候上电无法正常通讯  建议优先开底板电压 Wio重新用自己开关上电  如正常通行 则SYS处就算是Loss也是绿色图标 否则是红色
{
  static float timer=0;
  
  while(1)
  {
    float dt=Get_Cycle_T(1);
    int num=0;
    char accepted[50];
    while (Serial1.available()> 0)     
    {
        accepted[num] = Serial1.read();    
        //Serial.println(accepted[num]);       
        USART_IRQHandler(accepted[num]);     
        num++;
    }

    vTaskDelay(10 / portTICK_PERIOD_MS);//延时300ms
  }
  vTaskDelete(NULL);
}
#if 1
int wifi_scan()//直接连接
{
    float timer=0;
    Serial.println("scan start");
    WiFi.mode(WIFI_STA);
    WiFi.disconnect();      
    playNote( 'a',550);
    networkName_set=ssid;
    Serial.println("Auto Wifi Set: "+networkName_set+" "+password+" "+udpAddress);
    return 1;
}
#else
int wifi_scan()
{
    float timer=0;
    Serial.println("scan start");
    WiFi.mode(WIFI_STA);
    WiFi.disconnect();
    int n = WiFi.scanNetworks();
    int cnt_wifi_en=0;
    String wifi_buf[30];
    Serial.print("scan done:");
    Serial.println(n);
    #if 1
    if (n == 0) {
        Serial.println("no networks found");
    } else {
        for (int i = 0; i < n; ++i) {
            if(!generateJudgment(WiFi.SSID(i)))
            {  
                wifi_buf[cnt_wifi_en++]=WiFi.SSID(i);
            }
            delay(10);
        }
    }
    Serial.println("");
    #endif
    int Line_ST[2];
    int wifi_set=1;
    int button_sel=0;
    int cnt_txt=0;
    if(n>0){//--------按键选择Wifi
      tft_init_reset();     
 
      n=LIMIT(cnt_wifi_en,0,12);//限制显示
      while(wifi_set){
        timer+=0.02;
         
        start_loop();

        tft.setTextSize(1);
        tft.setTextColor(TFT_WHITE);
        tft.setCursor(165, 125);
        cnt_txt++;
        if(cnt_txt<50)
        tft.println(F("Link Robot.   "));
        else if(cnt_txt<100)
        tft.println(F("Link Robot..  "));
        else if(cnt_txt<150)
        tft.println(F("Link Robot... "));
        else if(cnt_txt<200)
        tft.println(F("Link Robot...."));
        else
        cnt_txt=0;

        static int WaytbuttonState_reg[5]={0};
        WaytbuttonState[0]=!digitalRead(WIO_5S_UP);
        WaytbuttonState[1]=!digitalRead(WIO_5S_DOWN);
        WaytbuttonState[2]=!digitalRead(WIO_5S_LEFT);
        WaytbuttonState[3]=!digitalRead(WIO_5S_RIGHT);
        WaytbuttonState[4]=!digitalRead(WIO_5S_PRESS);
  
        if(WaytbuttonState[1]&&!WaytbuttonState_reg[1]){
          timer=0;
          button_sel++;
        }
        if(WaytbuttonState[0]&&!WaytbuttonState_reg[0]){
          timer=0;
          button_sel--;    
        }
        button_sel=LIMIT(button_sel,0,n);

        for(int i=0;i<n;i++){
          Line_ST[0]=5;Line_ST[1]=5+Line_Height*i;
          content_buttion[i].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
          content_buttion[i].setTextSize(1);
          if(button_sel==i){
            content_buttion[i].fillSprite(TFT_CYAN);
            content_buttion[i].setTextColor(TFT_WHITE, TFT_CYAN);
            }
          else{
            content_buttion[i].fillSprite(TFT_DARKGREY);
            content_buttion[i].setTextColor(TFT_WHITE, TFT_DARKGREY);
          }
          content_buttion[i].drawString(wifi_buf[i], 0, 0, 2);
        }
        if(WaytbuttonState[4]&&!WaytbuttonState_reg[4]){
            playNote( 'a',550);
            networkName_set=wifi_buf[button_sel];

            if(networkName_set==ssid_sel[0]){
              networkName_set=ssid_sel[0];
              password=password_sel[0];
              udpAddress=udpAddress_sel[0];
              udpPort=udpPort_sel[0];
            }else{
              networkName_set=ssid_sel[1];
              password=password_sel[1];
              udpAddress=udpAddress_sel[1];
              udpPort=udpPort_sel[1];
            }

            Serial.println("Wifi Set: "+networkName_set+" "+password+" "+udpAddress+" "+udpPort);
            wifi_set=0;
            break;
        }else if(timer>2.5){
            playNote( 'a',550);
            networkName_set=ssid;
            Serial.println("Auto Wifi Set: "+networkName_set+" "+password+" "+udpAddress);
            wifi_set=0;
            break;
        }
        delay(20);
      }
    }
    return n;
}
#endif
void WiFiEvent(WiFiEvent_t event){//Wifi事件处理
    switch(event) {
      case SYSTEM_EVENT_STA_GOT_IP:
          // //When connected set 
          Serial.println("Connected");
          Serial.print("IP Address:");
          Serial.println(WiFi.localIP());
          Udp.begin(WiFi.localIP(),udpPort);
          connected = true;
          break;
      case SYSTEM_EVENT_STA_DISCONNECTED:
          Serial.println("WiFi lost connection");
          connected = false;
          robot_connect=0;
          break;
      default: break;
    }
}

void tft_init_reset(void)//主界面 显示刷屏
{
  tft.fillScreen(TFT_BLACK);
  tft.setRotation(3);//改变朝向
  
  graph1.setColorDepth(8);
  graph1.createSprite(G_W/2, G_H);//设置长度和高度
  //graph1.setRotation(3);//改变朝向  竖直朝向
  for(int i=0;i<6;i++){
    txt_att[i].setColorDepth(8);
    txt_att[i].createSprite(35, Line_Height);//字符长度和高度
    txt_att[i].fillSprite(TFT_BLUE); // Fill sprite with blue
    txt_att[i].setScrollRect(0, 0, 32, 64, TFT_BLUE);     // here we set scroll gap fill color to blue 这里只设置了背景颜色
    txt_att[i].setTextColor(TFT_WHITE); // White text, no background
    txt_att[i].setTextDatum(BR_DATUM);  // Bottom right coordinate datum
    
    content_att[i].setColorDepth(8);
    content_att[i].createSprite(40, Line_Height);//字符长度  高度
    content_att[i].fillSprite(TFT_DARKGREY);
    content_att[i].setScrollRect(0, 0, 40, Line_Height, TFT_DARKGREY); // Scroll the "Hello" in the first 40 pixels
    content_att[i].setTextColor(TFT_WHITE); // White text, no background
  }

  for(int i=0;i<2;i++){
    txt_mode[i].createSprite(40, Line_Height);
    txt_mode[i].fillSprite(TFT_BLACK);

    content_mode[i].setColorDepth(8);
    content_mode[i].createSprite(40, Line_Height);//字符长度  高度
    content_mode[i].fillSprite(TFT_NAVY);
    content_mode[i].setScrollRect(0, 0, 40, Line_Height, TFT_NAVY); // Scroll the "Hello" in the first 40 pixels
    content_mode[i].setTextColor(TFT_WHITE); // White text, no background
  }
  ////BAT RC 增加后搜索不出来Wifi
  for(int i=2;i<4;i++){
    txt_mode[i].setColorDepth(8);
    txt_mode[i].createSprite(38, Line_Height);//字符长度和高度
    txt_mode[i].fillSprite(TFT_DARKGREY); // Fill sprite with blue
    txt_mode[i].setScrollRect(0, 0, 38, Line_Height, TFT_BLUE);     // here we set scroll gap fill color to blue 这里只设置了背景颜色
    txt_mode[i].setTextColor(TFT_WHITE); // White text, no background
    txt_mode[i].setTextDatum(BR_DATUM);  // Bottom right coordinate datum

    content_mode[i].setColorDepth(8);
    content_mode[i].createSprite(40, Line_Height);//字符长度  高度
    content_mode[i].fillSprite(TFT_NAVY);
    content_mode[i].setScrollRect(0, 0, 40, Line_Height, TFT_NAVY); // Scroll the "Hello" in the first 40 pixels
    content_mode[i].setTextColor(TFT_WHITE); // White text, no background
  }

  //按键
  for(int i=0;i<12;i++){
    content_buttion[i].createSprite(100, Line_Height);
    content_buttion[i].fillSprite(TFT_BLACK);
  }

  // Create a sprite for the scrolling numbers 数字
  for(int i=0;i<12;i++){
    txt_q[i].setColorDepth(8);
    txt_q[i].createSprite(35, Line_Height);//字符长度和高度
    txt_q[i].fillSprite(TFT_BLUE); // Fill sprite with blue
    txt_q[i].setScrollRect(0, 0, 0, Line_Height, TFT_BLUE);     // here we set scroll gap fill color to blue 这里只设置了背景颜色
    txt_q[i].setTextColor(TFT_WHITE); // White text, no background
    txt_q[i].setTextDatum(BR_DATUM);  // Bottom right coordinate datum
  }
  
  content_q.setColorDepth(8);
  content_q.createSprite(40*4, Line_Height);//字符长度  高度
  content_q.fillSprite(TFT_DARKGREY);
  content_q.setScrollRect(0, 0, 40, Line_Height, TFT_DARKGREY); // Scroll the "Hello" in the first 40 pixels
  content_q.setTextColor(TFT_WHITE); // White text, no background

  //加速度计
  lis.begin(Wire1);
  lis.setOutputDataRate(LIS3DHTR_DATARATE_25HZ); //Data output rate
  lis.setFullScaleRange(LIS3DHTR_RANGE_2G); //Scale range set to 2g
  
  //通知
  stext2.setColorDepth(8);
  stext2.createSprite(160, Line_Height);//长度 宽度
  stext2.fillSprite(TFT_DARKGREY);
  stext2.setScrollRect(0, 0, 160, Line_Height, TFT_DARKGREY); // Scroll the "Hello" in the first 40 pixels
  stext2.setTextColor(TFT_WHITE); // White text, no background
  //机器人模型
  win_sel=robot_connect;
  robot_setup(robot_connect);
}

void param_read(){//读取配置文件
  myFile = SD.open("test.txt", FILE_WRITE);

  if (myFile) {
    Serial.print("Writing to test.txt...");
    myFile.println("testing 1, 2, 3.");
    myFile.close();
    Serial.println("done.");
  } else {
    Serial.println("error opening test.txt");
  }

  myFile = SD.open("wifi.txt", FILE_READ);
  char seps[]= ",";
  char  *stass;
  char *buf[100];
  int i=0;
  String comdata = "";
  if (myFile) {
    while (myFile.available()) {
      comdata += char(myFile.read());//方便读取使用
    }
    myFile.close();
    Serial.println("Param: "+comdata);
    char * p=(char*)comdata.c_str();
    stass=strtok(p,seps);
    while( stass != NULL ) {
      buf[i] = stass;
      ++i;
      stass = strtok(NULL, seps);
    }

    ssid_sel[0] = buf[0];
    password_sel[0]= buf[1];
    udpAddress_sel[0] = buf[2];
    udpPort_sel[0] = atoi(buf[3]);
    Serial.println(ssid_sel[0]);
    Serial.println(password_sel[0]);
    Serial.println(udpAddress_sel[0]);
    Serial.println(udpPort_sel[0]);

    ssid_sel[1] = buf[4];
    password_sel[1] = buf[5];
    udpAddress_sel[1] = buf[6];
    udpPort_sel[1] = atoi(buf[7]);
    Serial.println(ssid_sel[1]);
    Serial.println(password_sel[1]);
    Serial.println(udpAddress_sel[1]);
    Serial.println(udpPort_sel[1]);
  }else {
    Serial.println("error opening wifi.txt");
  }
}

void setup()//系统初始化
{
  Serial.begin(115200);
  delay(500);
  Serial1.begin(115200);
  delay(500);
  Serial.println("--------------------------------");
  tft.begin();
  tft.init();
  tft.setRotation(3);

  if (!SD.begin(SDCARD_SS_PIN, SDCARD_SPI)) {
    tft.println(F("    No SD Card!    "));
    while (1);
  }
  
  drawImage<uint8_t>("start.bmp", 0, 0); //显示开机图片 需要在SD卡中放入
  tft.setFreeFont(FSB18);                              // Select the font
  tft.drawString("Quad-Terminal", 45, 105, GFXFF);  // Draw t
  tft.drawString("Designed By Tinymal-RC", 100, 140, FONT2);  // Draw t
  delay(4000);
  tft.setFreeFont(FSB9);  

  param_read();
  
  tft_init_reset();//

  beep_setup();
  // 按键
  pinMode(WIO_KEY_A, INPUT); 
  pinMode(WIO_KEY_B, INPUT); 
  pinMode(WIO_KEY_C, INPUT); 
  pinMode(WIO_5S_UP, INPUT_PULLUP);
  pinMode(WIO_5S_DOWN, INPUT_PULLUP);
  pinMode(WIO_5S_LEFT, INPUT_PULLUP);
  pinMode(WIO_5S_RIGHT, INPUT_PULLUP);
  pinMode(WIO_5S_PRESS, INPUT_PULLUP);

#if !FAST_TEST
  start_setup();
  tft.setFreeFont(FSB9);  
  tft.drawString("Searching Wifi...", 95, 110, GFXFF);  
  wifi_scan();

  WiFi.disconnect();
  WiFi.mode(WIFI_STA);
  WiFi.onEvent(WiFiEvent);
  const char*  ssid_set = networkName_set.c_str();
  Serial.println("Wifi Link: "+networkName_set+" "+password+" "+udpAddress+" "+udpPort);
  WiFi.begin(ssid_set, password);
  int cnt_try=0;
  int en_draw=1;
  tft_init_reset();
 #endif
 
  robot_setup(0);
  #if USE_XBOX_RC
   setup_xbox();
  #endif

//-------------启动线程 最大1024堆栈-----------
  xTaskCreate(
  taskOne,   /* Task function. */
  "UDP", /* String with name of task. */
  1024,     /* Stack size in bytes. */
  NULL,      /* Parameter passed as input of the task */
  1,         /* Priority of the task. */
  NULL);     /* Task handle. */
  
#if 1
  xTaskCreate(
  taskTwo,   /* Task function. */
  "UART", /* String with name of task. */
  128,     /* Stack size in bytes. */
  NULL,      /* Parameter passed as input of the task */
  0,         /* Priority of the task. */
  NULL);     /* Task handle. */
#endif
}
 
void loop(){//-----------------主循环 依据窗口显示-----------------------
  switch(win_sel)
  {
    case 0:main_win();break;
    case 1:nav_win();break;
  }
}

void nav_win()//------------------上帝视角--------------------------
{
  static int button_sel=0;
  float dt=Get_Cycle_T(0);
  static int buttonState_reg[3]={0};
  static int WaytbuttonState_reg[5]={0};
  robot_loss_cnt+=dt;
  if(robot_loss_cnt>3){
    robot_loss_cnt=0;
    robot_connect=0;
  }
  loss_cnt_io+=dt;
  if(loss_cnt_io>3){
    loss_cnt_io=0;
    connect_io=0;
  }

  buttonState[0] = !digitalRead(WIO_KEY_A);
  buttonState[1] = !digitalRead(WIO_KEY_B);
  buttonState[2] = !digitalRead(WIO_KEY_C);

  WaytbuttonState[0]=!digitalRead(WIO_5S_UP);
  WaytbuttonState[1]=!digitalRead(WIO_5S_DOWN);
  WaytbuttonState[2]=!digitalRead(WIO_5S_LEFT);
  WaytbuttonState[3]=!digitalRead(WIO_5S_RIGHT);
  WaytbuttonState[4]=!digitalRead(WIO_5S_PRESS);

  float x_values, y_values, z_values;
  x_values = lis.getAccelerationX();
  y_values = lis.getAccelerationY();
  z_values = lis.getAccelerationZ();

  int kp_att[3]={45,20*0,20*0};//ROL PIT YAW  显示数字孪生
  robot_loop(x_values*kp_att[0],y_values*kp_att[1],z_values*kp_att[2],dt);

 if(WaytbuttonState[3]&&!WaytbuttonState_reg[3])
      button_sel++;
    if(WaytbuttonState[2]&&!WaytbuttonState_reg[2])
      button_sel--;   

    if(WaytbuttonState[0]&&!WaytbuttonState_reg[0])//up
    {
      if(button_sel==3)button_sel=0;
      else if(button_sel==4)button_sel=1;
      else if(button_sel==5)button_sel=2;  
      else if(button_sel==6)button_sel=3;
      else if(button_sel==7)button_sel=4;
      else if(button_sel==8)button_sel=5;  
    }
    if(WaytbuttonState[1]&&!WaytbuttonState_reg[1])//down
    {
      if(button_sel==0)button_sel=3;
      else if(button_sel==1)button_sel=4;
      else if(button_sel==2)button_sel=5;  
      else if(button_sel==3)button_sel=6;
      else if(button_sel==4)button_sel=7;
      else if(button_sel==5)button_sel=8;  
    } 
    button_sel=LIMIT(button_sel,0,2-1);

    int Line_ST[2];
    int grid_w=80;
//按键绘制
    Line_ST[0]=220;Line_ST[1]=220;//00
    content_buttion[0].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==0)
      content_buttion[0].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[0].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[0].drawString("  System View   ", 0, 0, 2);

     if(WaytbuttonState[4]==1&&WaytbuttonState_reg[4]==0)
    {
      if(button_sel==0){
        playNote( 'A',550);
        tft_init_reset();//
        robot_setup(0);
        win_sel=0;
      }
    }    

//遥控状态机
  static int flag_llrr=0;
  static float shut_down_cnt=0;
  static float motor_init_cnt=0;
  static int cnt_switch_z[2]={0};
  if(ocu_mode==1){//机器人操作模式

      //B 2 1 0
      switch(nav_rx.robot_mode){
        case M_SAFE:
          if(buttonState[0]&&!buttonState_reg[0]){//蹲起站立
          if(flag_llrr)
          {
          ocu.key_ll=1;
          ocu.key_rr=0;
          }
          else{
          ocu.key_ll=0;
          ocu.key_rr=1;
          }
          flag_llrr=!flag_llrr;
          }

          if(buttonState[2])
          motor_init_cnt+=dt;
          else
          motor_init_cnt=0;

          if(motor_init_cnt>1){
          motor_init_cnt=0;
          ocu.key_x=1;
          }else if(ocu.key_x==1&&!buttonState[2])
          ocu.key_x=0;

          ocu.key_a=ocu.key_b=0;
          ocu.rc_spd_w[0]=0;ocu.rc_spd_w[1]=0;
          ocu.rc_att_w[0]=0;ocu.rc_att_w[1]=0;
        break;
        case M_STAND_RC:
          if(WaytbuttonState[0])//up
          ocu.rc_att_w[0]=0.8;
          else if(WaytbuttonState[1])//down
          ocu.rc_att_w[0]=-0.8;
          else
          ocu.rc_att_w[0]=0;

          if(WaytbuttonState[2])//left
          ocu.rate_yaw_w=1;
          else if(WaytbuttonState[3])//right
          ocu.rate_yaw_w=-1;
          else
          ocu.rate_yaw_w=0;

          if(connect_io){//连接IO板子
            ocu.rc_att_w[0]=rc[0];
            ocu.rate_yaw_w=rc[1];
          }

          if(buttonState[0]&&!buttonState_reg[0]){//蹲起站立
            cnt_switch_z[1]=1;
            cnt_switch_z[0]=0;
            if(flag_llrr)
            {
            ocu.key_ll=1;
            ocu.key_rr=0;
            }
            else{
            ocu.key_ll=0;
            ocu.key_rr=1;
            }
            flag_llrr=!flag_llrr;
          }
          if(cnt_switch_z[1])
              cnt_switch_z[0]++;
          if(cnt_switch_z[0]>3)
          {
              ocu.key_ll=0;
              ocu.key_rr=0;
              cnt_switch_z[1]=0;
              cnt_switch_z[0]=0;
          }

          if(buttonState[1])
            ocu.key_a=1;
          else
            ocu.key_a=0;

          if(buttonState[2])
            ocu.key_lr=1;
          else
            ocu.key_lr=0;

          ocu.rc_spd_w[0]=0;ocu.rc_spd_w[1]=0;
        break;
        case M_C_TROT:
          if(WaytbuttonState[0])//up
          ocu.rc_spd_w[0]=0.8;
          else if(WaytbuttonState[1])//down
          ocu.rc_spd_w[0]=-0.8;
          else
          ocu.rc_spd_w[0]=0;

          if(WaytbuttonState[2])//left
          ocu.rate_yaw_w=0.5;
          else if(WaytbuttonState[3])//right
          ocu.rate_yaw_w=-0.5;
          else
          ocu.rate_yaw_w=0;

          if(connect_io){//连接IO板子
            ocu.rc_spd_w[0]=rc[0];
            ocu.rate_yaw_w=rc[1];
          }

          if(buttonState[1])
            ocu.key_a=1;
          else
            ocu.key_a=0;

          if(buttonState[0])
            ocu.key_b=1;
          else
            ocu.key_b=0;  
          ocu.rc_att_w[0]=0;ocu.rc_att_w[1]=0;
          ocu.key_ll=ocu.key_rr=0;
        break;
        case M_TROT:
          
          if(WaytbuttonState[0])//up
          ocu.rc_spd_w[0]=0.8;
          else if(WaytbuttonState[1])//down
          ocu.rc_spd_w[0]=-0.8;
          else
          ocu.rc_spd_w[0]=0;

          if(buttonState[2])//left
          ocu.rate_yaw_w=0.7;
          else if(buttonState[3])//right
          ocu.rate_yaw_w=-0.7;
          else
          ocu.rate_yaw_w=0;

          if(buttonState[0])
            ocu.key_lr=1;
          else
            ocu.key_lr=0;

          if(WaytbuttonState[2])
            ocu.rc_spd_w[1]=-1;
          else if(WaytbuttonState[1])
            ocu.rc_spd_w[1]=1;
          else 
            ocu.rc_spd_w[1]=0;

          if(connect_io){//连接IO板子
            ocu.rc_spd_w[0]=rc[0];
            ocu.rc_spd_w[1]=rc[1];
          }

          ocu.rc_att_w[0]=0;ocu.rc_att_w[1]=0;
          ocu.key_ll=ocu.key_rr=0;
        break;
      }
    
      if(WaytbuttonState[4])//断电
        shut_down_cnt+=dt;
      else
        shut_down_cnt=0;
      
      if(shut_down_cnt>2){
        shut_down_cnt=0;
        ocu.key_ud=-1;
      }else if(ocu.key_ud==-1&&!WaytbuttonState[4])
        ocu.key_ud=0;
  }
  
  for(int i=0;i<5;i++){
    WaytbuttonState_reg[i]=WaytbuttonState[i];
  }
  for(int i=0;i<3;i++){
    buttonState_reg[i]=buttonState[i];
  }
  delay(10);
}

void main_win()//---------主界面
{
  float dt=Get_Cycle_T(0);
  static int buttonState_reg[3]={0};
  static int WaytbuttonState_reg[5]={0};;
  robot_loss_cnt+=dt;
  if(robot_loss_cnt>3){
    robot_loss_cnt=0;
    robot_connect=0;
  }
  loss_cnt_io+=dt;
  if(loss_cnt_io>3){
    loss_cnt_io=0;
    connect_io=0;
    Serial1.begin(115200);
  }

#if 1
   //------------KEY
  #if USE_XBOX_RC
   loop_xbox();
  #endif
  
  buttonState[0] = !digitalRead(WIO_KEY_A);
  buttonState[1] = !digitalRead(WIO_KEY_B);
  buttonState[2] = !digitalRead(WIO_KEY_C);

  WaytbuttonState[0]=!digitalRead(WIO_5S_UP);
  WaytbuttonState[1]=!digitalRead(WIO_5S_DOWN);
  WaytbuttonState[2]=!digitalRead(WIO_5S_LEFT);
  WaytbuttonState[3]=!digitalRead(WIO_5S_RIGHT);
  WaytbuttonState[4]=!digitalRead(WIO_5S_PRESS);

  float x_values, y_values, z_values;
  x_values = lis.getAccelerationX();
  y_values = lis.getAccelerationY();
  z_values = lis.getAccelerationZ();

//-------------------标定状态机
  static int lsm=0;
  static float timer[10]={0};
  switch(lsm){
    case 0:
    if(buttonState[0])
     timer[0]+=dt;
    else
     timer[0]=0;
    if(buttonState[1]&&ocu_mode==0)
     timer[1]+=dt;
    else
     timer[1]=0;
    if(buttonState[2]&&ocu_mode==0)
     timer[2]+=dt;
    else
     timer[2]=0;

    if(timer[2]>4.5)//cal imu
    {
      nav_rx.cal_imu=1;
      timer[2]=0;
      Serial.println("Cal_IMU");
      notice=CAL_IMU;
      playNote( 'c',550);
    }
    if(timer[1]>4.5)//cal Q
    {
      nav_rx.cal_q=1;
      timer[1]=0;
      Serial.println("Cal_Q");
      notice=CAL_Q;
      ocu.key_y=1;
      playNote( 'c',550);
    } 
    if(timer[0]>3)//修改控制模式
    {
      timer[0]=0;
      Serial.println("Button Mode Change");
      notice=B_MODE;
      if(button_mode==BUTTON_RC){
        ocu_mode=0;
        button_mode=BUTTON_WIN;
      }
      else{
        ocu_mode=1;
        button_mode=BUTTON_RC;
      }
     playNote( 'C',550);
    } 
    break;
  }
#endif
 //--------------显示UI
   int Line_ST[2]={0,Line_Height*2};
#if 1
  float size=1;
  #if 1
   //  绘制波形Line1
  graph1.drawFastVLine(G_W/2-1, G_H/2-nav_rx.att_now[0]*size, 2, TFT_YELLOW); // draw 2 pixel point on graph
  graph1.pushSprite(0, 120);//设置位置
  graph1.scroll(-1, 0); // scroll graph 1 pixel left, 0 up/down
  grid[0]++;
  if (grid[0] >= 20) {
      // Draw a vertical line if we have scrolled 10 times (10 pixels)
      grid[0] = 0;
      graph1.drawFastVLine(G_W/2-1, 0, G_H+1, TFT_NAVY); // draw line on graph
  } else {
      // Otherwise draw points spaced 10 pixels for the horizontal grid lines
      for (int p = 0; p <= G_H; p += 20) {
          graph1.drawPixel(G_W/2, p, TFT_NAVY);
      }
  }

  //  绘制波形Line2
  graph1.drawFastVLine(G_W/2-1, G_H/2-nav_rx.att_now[1]*size, 2, TFT_RED); // draw 2 pixel point on graph
  graph1.pushSprite(0, 120);//设置位置
  graph1.scroll(-1, 0); // scroll graph 1 pixel left, 0 up/down
  grid[1]++;
  if (grid[1] >= 20) {
      // Draw a vertical line if we have scrolled 10 times (10 pixels)
      grid[1] = 0;
      graph1.drawFastVLine(G_W/2-1, 0, G_H+1, TFT_NAVY); // draw line on graph
  } else {
      // Otherwise draw points spaced 10 pixels for the horizontal grid lines
      for (int p = 0; p <= G_H; p += 20) {
          graph1.drawPixel(G_W/2, p, TFT_NAVY);
      }
  }
  #endif
  

  int kp_att[3]={45,20*0,20*0};//ROL PIT YAW  显示数字孪生
  robot_loop(x_values*kp_att[0],y_values*kp_att[1],z_values*kp_att[2],dt);
#endif

#if 1
  //--------------------------底部系统通知------------------------------------------
  static int state_notice=0;
  static char string_n[30]="";
  stext2.scroll(3);     // scroll stext 1 pixel right, up/down default is 0
  stext2.pushSprite(160, 220);
  switch(state_notice){
  case 0:
    if(notice==CAL_Q)
    {
      tcount = 160;
      state_notice++;
      stext2.drawString("Calib Q Zero! ", 0, 0, 2); // draw at 6,0 in sprite, font 2
    }
    if(notice==CAL_IMU)
    {
      tcount = 160;
      state_notice++;
      stext2.drawString("Calib IMU Off! ", 0, 0, 2); // draw at 6,0 in sprite, font 2
    }
    if(notice==B_MODE)
    {
      tcount = 160;
      state_notice++;
      if(button_mode==BUTTON_WIN)
        stext2.drawString("Mode Into System! ", 0, 0, 2); // draw at 6,0 in sprite, font 2
      if(button_mode==BUTTON_RC)
        stext2.drawString("Mode Into RC-Remote! ", 0, 0, 2); // draw at 6,0 in sprite, font 2
    }
  break;
  case 1:
    tcount=tcount-3;
    if (tcount <= 0) {
        notice=0;
        state_notice=0;    
        if(ocu.key_y) ocu.key_y=0;
    }
  break;
  }

  //--------------------------绘制姿态角度Txt
  //ATT
  Line_ST[0]=0;Line_ST[1]=0;
  draw_content(content_att[0],"PIT",Line_ST[0],Line_ST[1]);
  draw_data(txt_att[0],nav_rx.att_now[0],Line_ST[0]+40,Line_ST[1]); 
  Line_ST[0]=80;Line_ST[1]=0;
  draw_content(content_att[1],"ROL",Line_ST[0],Line_ST[1]);
  draw_data(txt_att[1],nav_rx.att_now[1],Line_ST[0]+40,Line_ST[1]); 
  Line_ST[0]=80*2;Line_ST[1]=0;
  draw_content(content_att[2],"YAW",Line_ST[0],Line_ST[1]);
  draw_data(txt_att[2],nav_rx.att_now[2],Line_ST[0]+40,Line_ST[1]); 
  //RATE
  Line_ST[0]=0;Line_ST[1]=Line_Height;
  draw_content(content_att[3],"DPIT",Line_ST[0],Line_ST[1]);
  draw_data(txt_att[3],nav_rx.datt_now[0],Line_ST[0]+40,Line_ST[1]); 
  Line_ST[0]=80;Line_ST[1]=Line_Height;
  draw_content(content_att[4],"DROL",Line_ST[0],Line_ST[1]);
  draw_data(txt_att[4],nav_rx.datt_now[1],Line_ST[0]+40,Line_ST[1]); 
  Line_ST[0]=80*2;Line_ST[1]=Line_Height;
  draw_content(content_att[5],"DYAW",Line_ST[0],Line_ST[1]);
  draw_data(txt_att[5],nav_rx.datt_now[2],Line_ST[0]+40,Line_ST[1]); 
  
  //--------------------------模式
  static float mode_t[3]={0};
  static int flag_mode[3]={0};
  Line_ST[0]=80*3;Line_ST[1]=0;
  draw_content(content_mode[0],"SYS",Line_ST[0],Line_ST[1]);
  if(!robot_connect)
  {
      mode_t[0]+=dt;
      if(mode_t[0]>1){
      mode_t[0]=0;
      flag_mode[0]=!flag_mode[0];
      }
      if(flag_mode[0]){
        txt_mode[0].setTextSize(1);
        txt_mode[0].fillSprite(TFT_BLACK);
        txt_mode[0].setTextColor(TFT_WHITE, TFT_BLACK);
        txt_mode[0].drawString("LOSS", 0, 0, 2);
      }else{ 
        txt_mode[0].setTextSize(1);
        if(connect_io){
          txt_mode[0].fillSprite(TFT_GREEN);
          txt_mode[0].setTextColor(TFT_WHITE, TFT_GREEN);
        }else{
          txt_mode[0].fillSprite(TFT_RED);
          txt_mode[0].setTextColor(TFT_WHITE, TFT_RED);   
        }
        txt_mode[0].drawString("LOSS", 0, 0, 2);
      }
      draw_content(txt_mode[0],"LOSS",Line_ST[0]+40,Line_ST[1]);
  }else{
      txt_mode[0].pushSprite(Line_ST[0]+40,Line_ST[1]);//设置坐标
      txt_mode[0].setTextSize(1);
      if(connect_io){
      txt_mode[0].fillSprite(TFT_GREEN);
      txt_mode[0].setTextColor(TFT_BLACK, TFT_GREEN);
      }else{
      txt_mode[0].fillSprite(TFT_RED);
      txt_mode[0].setTextColor(TFT_BLACK, TFT_RED);
      }

      switch(nav_rx.robot_mode){
        case M_SAFE:txt_mode[0].drawString("SAFE", 0, 0, 2);break;
        case M_STAND_RC:txt_mode[0].drawString("STAND", 0, 0, 2);break;
        case M_C_TROT:txt_mode[0].drawString("CTROT", 0, 0, 2);break;
        case M_TROT:txt_mode[0].drawString("TROT", 0, 0, 2);break;
      }
      
   }

  Line_ST[0]=80*3;Line_ST[1]=Line_Height;
  draw_content(content_mode[1],"OCU",Line_ST[0],Line_ST[1]);
  if(button_mode==BUTTON_WIN){
        txt_mode[1].pushSprite(Line_ST[0]+40,Line_ST[1]);//设置坐标
        txt_mode[1].setTextSize(1);
        txt_mode[1].fillSprite(TFT_YELLOW);
        txt_mode[1].setTextColor(TFT_BLACK, TFT_YELLOW);
        txt_mode[1].drawString("WIN", 0, 0, 2);
    }
  if(button_mode==BUTTON_RC){
        txt_mode[1].pushSprite(Line_ST[0]+40,Line_ST[1]);//设置坐标
        txt_mode[1].setTextSize(1);
        txt_mode[1].fillSprite(TFT_PURPLE);
        txt_mode[1].setTextColor(TFT_WHITE, TFT_PURPLE);
        if(rc_connect)
          txt_mode[1].drawString("R-360", 0, 0, 2);
        else
          txt_mode[1].drawString("R-Key", 0, 0, 2);
    }
 
  //--------------------------BAT RC电量
  Line_ST[0]=80*2;Line_ST[1]=Line_Height*2;
  //bat=45;
  draw_content(content_mode[2],"B-IO",Line_ST[0],Line_ST[1]);
  if(bat>10){
    txt_mode[2].pushSprite(Line_ST[0]+40,Line_ST[1]);                  //设置坐标
    txt_mode[2].fillSprite(TFT_GREEN); // Fill sprite with blue  
    txt_mode[2].drawNumber(abs(bat), 28, Line_Height, 2);  // plot value in font 2 更新数字 
  }
  else{
    txt_mode[2].pushSprite(Line_ST[0]+40,Line_ST[1]);                  //设置坐标
    txt_mode[2].fillSprite(TFT_RED); // Fill sprite with blue  
    txt_mode[2].drawNumber(abs(bat), 28, Line_Height, 2);  // plot value in font 2 更新数字
  }
 
  Line_ST[0]=80*3;Line_ST[1]=Line_Height*2;
  draw_content(content_mode[3],"B-RO",Line_ST[0],Line_ST[1]);
  txt_mode[3].pushSprite(Line_ST[0]+40,Line_ST[1]);                  //设置坐标
  txt_mode[3].fillSprite(TFT_RED); // Fill sprite with blue  
  txt_mode[3].drawNumber(abs(bat), 28, Line_Height, 2);  // plot value in font 2 更新数字

  //--------------------------绘制关节角度TXT
  int Dis=40;
  Line_ST[0]=0;Line_ST[1]=Line_Height*3;//FL
  content_q.pushSprite(Line_ST[0],Line_ST[1]);           //设置坐标
  content_q.drawString("FL", 6, 0, 2);  // draw at 6,0 in sprite, font 2
  draw_data(txt_q[0],nav_rx.sita_now[2][0],Line_ST[0],Line_ST[1]+Line_Height);
  draw_data(txt_q[1],nav_rx.sita_now[2][1],Line_ST[0],Line_ST[1]+Line_Height*2);
  draw_data(txt_q[2],nav_rx.sita_now[2][2],Line_ST[0],Line_ST[1]+Line_Height*3);
  Line_ST[0]=Dis;Line_ST[1]=Line_Height*3;//FR
  content_q.drawString("FR", 6+Dis, 0, 2);  // draw at 6,0 in sprite, font 2
  draw_data(txt_q[3],nav_rx.sita_now[0][0],Line_ST[0],Line_ST[1]+Line_Height);
  draw_data(txt_q[4],nav_rx.sita_now[0][1],Line_ST[0],Line_ST[1]+Line_Height*2);
  draw_data(txt_q[5],nav_rx.sita_now[0][2],Line_ST[0],Line_ST[1]+Line_Height*3);
  Line_ST[0]=Dis*2;Line_ST[1]=Line_Height*3;//HR
  content_q.drawString("HR", 6+Dis*2, 0, 2);  // draw at 6,0 in sprite, font 2
  draw_data(txt_q[6],nav_rx.sita_now[1][0],Line_ST[0],Line_ST[1] +Line_Height);
  draw_data(txt_q[7],nav_rx.sita_now[1][1],Line_ST[0],Line_ST[1] +Line_Height*2);
  draw_data(txt_q[8],nav_rx.sita_now[1][2],Line_ST[0],Line_ST[1] +Line_Height*3);
  Line_ST[0]=Dis*3;Line_ST[1]=Line_Height*3;//HL
  content_q.drawString("HL", 6+Dis*3, 0, 2);  // draw at 6,0 in sprite, font 2
  draw_data(txt_q[9], nav_rx.sita_now[3][0],Line_ST[0],Line_ST[1]+Line_Height);
  draw_data(txt_q[10],nav_rx.sita_now[3][1],Line_ST[0],Line_ST[1]+Line_Height*2);
  draw_data(txt_q[11],nav_rx.sita_now[3][2],Line_ST[0],Line_ST[1]+Line_Height*3);
  #endif


 //--------------------------用户按键逻辑
  static int gui_no_wifi_sel[10][10];
  static int button_sel=0;
  static int button_reg=0;
  int grid_w=55;
  if(robot_connect){//button_mode==BUTTON_WIN){//参数配置
    if(button_mode==BUTTON_WIN){
      if(WaytbuttonState[3]&&!WaytbuttonState_reg[3])
        button_sel++;
      if(WaytbuttonState[2]&&!WaytbuttonState_reg[2])
        button_sel--;   

      if(WaytbuttonState[0]&&!WaytbuttonState_reg[0])//up
      {
        if(button_sel==3)button_sel=0;
        else if(button_sel==4)button_sel=1;
        else if(button_sel==5)button_sel=2;  
        else if(button_sel==6)button_sel=3;
        else if(button_sel==7)button_sel=4;
        else if(button_sel==8)button_sel=5;  
      }
      if(WaytbuttonState[1]&&!WaytbuttonState_reg[1])//down
      {
        if(button_sel==0)button_sel=3;
        else if(button_sel==1)button_sel=4;
        else if(button_sel==2)button_sel=5;  
        else if(button_sel==3)button_sel=6;
        else if(button_sel==4)button_sel=7;
        else if(button_sel==5)button_sel=8;  
      } 
    }
    button_sel=LIMIT(button_sel,0,9-1);
    
    Line_ST[0]=160;Line_ST[1]=160;//00
    content_buttion[0].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==0)
      content_buttion[0].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[0].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[0].drawString(" CAL Q ", 0, 0, 2);
  
    Line_ST[0]=160+grid_w;Line_ST[1]=160;//01
    content_buttion[1].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==1)
      content_buttion[1].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[1].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[1].drawString("CAL IMU", 0, 0, 2);

    Line_ST[0]=160+grid_w*2;Line_ST[1]=160;//02
    content_buttion[2].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==2)
      content_buttion[2].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[2].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[2].drawString("  GOD  ", 0, 0, 2);

    Line_ST[0]=160;Line_ST[1]=160+Line_Height+3;//10
    content_buttion[3].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==3)
      content_buttion[3].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[3].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[3].drawString(" SLAM  ", 0, 0, 2);  

    Line_ST[0]=160+grid_w;Line_ST[1]=160+Line_Height+3;//1
    content_buttion[4].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==4)
      content_buttion[4].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[4].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[4].drawString("PARAM ", 0, 0, 2);  

    Line_ST[0]=160+grid_w*2;Line_ST[1]=160+Line_Height+3;//12
    content_buttion[5].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==5)
      content_buttion[5].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[5].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[5].drawString(" CURVE ", 0, 0, 2);  

    Line_ST[0]=160;Line_ST[1]=160+Line_Height*2+3*2;//20
    content_buttion[6].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==6)
      content_buttion[6].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[6].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[6].drawString(" CABIN ", 0, 0, 2);  
//
    Line_ST[0]=160+grid_w;Line_ST[1]=160+Line_Height*2+3*2;//21
    content_buttion[7].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==7)
      content_buttion[7].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[7].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[7].drawString("  ARM  ", 0, 0, 2);  

    Line_ST[0]=160+grid_w*2;Line_ST[1]=160+Line_Height*2+3*2;//22
    content_buttion[8].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==8)
      content_buttion[8].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[8].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[8].drawString("RECORD ", 0, 0, 2);  

    if(WaytbuttonState[4]==1&&WaytbuttonState_reg[4]==0&&button_mode==BUTTON_WIN)
    {
      switch(button_sel)
      {
      case 0:
      nav_rx.cal_q=1;
      Serial.println("Cal_Q");
      notice=CAL_Q;
      ocu.key_y=1;
      playNote( 'c',550);
      break;
      case 1:
      nav_rx.cal_imu=1;
      Serial.println("Cal_IMU");
      notice=CAL_IMU;
      playNote( 'c',550);
      break;
      case 2:
      playNote( 'A',550);
      tft_init_reset();//
      robot_setup(1);
      win_sel=1;
      ocu_mode=1;
      button_mode==BUTTON_RC;
      break;
      }    
    }
  }else{//end win sel-------------------------------
    if(WaytbuttonState[3]&&!WaytbuttonState_reg[3])
      button_sel++;
    if(WaytbuttonState[2]&&!WaytbuttonState_reg[2])
      button_sel--;   

    if(WaytbuttonState[0]&&!WaytbuttonState_reg[0])//up
    {
      if(button_sel==3)button_sel=0;
      else if(button_sel==4)button_sel=1;
      else if(button_sel==5)button_sel=2;  
      else if(button_sel==6)button_sel=3;
      else if(button_sel==7)button_sel=4;
      else if(button_sel==8)button_sel=5;  
    }
    if(WaytbuttonState[1]&&!WaytbuttonState_reg[1])//down
    {
      if(button_sel==0)button_sel=3;
      else if(button_sel==1)button_sel=4;
      else if(button_sel==2)button_sel=5;  
      else if(button_sel==3)button_sel=6;
      else if(button_sel==4)button_sel=7;
      else if(button_sel==5)button_sel=8;  
    } 
    button_sel=LIMIT(button_sel,0,2-1);

    Line_ST[0]=160;Line_ST[1]=160;//00
    content_buttion[0].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==0)
      content_buttion[0].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[0].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[0].drawString(" Linking Wifi ", 0, 0, 2);

   Line_ST[0]=160+grid_w*1.5;Line_ST[1]=160;//00
    content_buttion[1].pushSprite(Line_ST[0],Line_ST[1]);//设置坐标
    if(button_sel==1)
      content_buttion[1].setTextColor(TFT_WHITE, TFT_RED);
    else
      content_buttion[1].setTextColor(TFT_WHITE, TFT_DARKGREY);
    content_buttion[1].drawString("  God View   ", 0, 0, 2);

     if(WaytbuttonState[4]==1&&WaytbuttonState_reg[4]==0)
    {
      if(button_sel==0){
        playNote( 'c',550);
        wifi_search=1;
        wifi_scan();
        WiFi.disconnect();
        WiFi.mode(WIFI_STA);
        WiFi.onEvent(WiFiEvent);
        const char*  ssid_set = networkName_set.c_str();
        WiFi.begin(ssid_set, password);
        tft_init_reset();//
        wifi_search=0;
      }
      if(button_sel==1){
        playNote( 'A',550);
        tft_init_reset();//
        robot_setup(1);
        win_sel=1;
      }
    }    
  }

  //--------------------------控制与遥控
  static int flag_llrr=0;
  static float shut_down_cnt=0;
  static float motor_init_cnt=0;
  static int cnt_switch_z[2]={0};
  if(ocu_mode==1){//机器人操作模式

      //B 2 1 0
      switch(nav_rx.robot_mode){
        case M_SAFE:
          if(buttonState[0]&&!buttonState_reg[0]){//蹲起站立
          if(flag_llrr)
          {
          ocu.key_ll=1;
          ocu.key_rr=0;
          }
          else{
          ocu.key_ll=0;
          ocu.key_rr=1;
          }
          flag_llrr=!flag_llrr;
          }

          if(buttonState[2])
          motor_init_cnt+=dt;
          else
          motor_init_cnt=0;

          if(motor_init_cnt>1){
          motor_init_cnt=0;
          ocu.key_x=1;
          }else if(ocu.key_x==1&&!buttonState[2])
          ocu.key_x=0;

          ocu.key_a=ocu.key_b=0;
          ocu.rc_spd_w[0]=0;ocu.rc_spd_w[1]=0;
          ocu.rc_att_w[0]=0;ocu.rc_att_w[1]=0;
        break;
        case M_STAND_RC:
          if(WaytbuttonState[0])//up
          ocu.rc_att_w[0]=0.8;
          else if(WaytbuttonState[1])//down
          ocu.rc_att_w[0]=-0.8;
          else
          ocu.rc_att_w[0]=0;

          if(WaytbuttonState[2])//left
          ocu.rate_yaw_w=1;
          else if(WaytbuttonState[3])//right
          ocu.rate_yaw_w=-1;
          else
          ocu.rate_yaw_w=0;
        
          if(connect_io){//连接IO板子
            ocu.rc_att_w[0]=rc[0];
            ocu.rate_yaw_w=-rc[1];
          }
          
          if(buttonState[0]&&!buttonState_reg[0]){//蹲起站立
            cnt_switch_z[1]=1;
            cnt_switch_z[0]=0;
            if(flag_llrr)
            {
            ocu.key_ll=1;
            ocu.key_rr=0;
            }
            else{
            ocu.key_ll=0;
            ocu.key_rr=1;
            }
            flag_llrr=!flag_llrr;
          }
          if(cnt_switch_z[1])
              cnt_switch_z[0]++;
          if(cnt_switch_z[0]>3)
          {
              ocu.key_ll=0;
              ocu.key_rr=0;
              cnt_switch_z[1]=0;
              cnt_switch_z[0]=0;
          }

          if(buttonState[1])
            ocu.key_a=1;
          else
            ocu.key_a=0;

          if(buttonState[2])
            ocu.key_lr=1;
          else
            ocu.key_lr=0;

          ocu.rc_spd_w[0]=0;ocu.rc_spd_w[1]=0;
        break;
        case M_C_TROT:
          if(connect_io){//连接IO板子
            ocu.rc_spd_w[0]=rc[0];
            ocu.rate_yaw_w=-rc[1];
            if(buttonState[2])
              ocu.rc_spd_w[1]=-1;
            else if(buttonState[1])
              ocu.rc_spd_w[1]=1;
            else 
              ocu.rc_spd_w[1]=0;
          }else{
            if(WaytbuttonState[0])//up
            ocu.rc_spd_w[0]=0.8;
            else if(WaytbuttonState[1])//down
            ocu.rc_spd_w[0]=-0.8;
            else
            ocu.rc_spd_w[0]=0;
  
            if(WaytbuttonState[2])//left
            ocu.rate_yaw_w=0.5;
            else if(WaytbuttonState[3])//right
            ocu.rate_yaw_w=-0.5;
            else
            ocu.rate_yaw_w=0;
          }

          if(buttonState[1])
            ocu.key_a=1;
          else
            ocu.key_a=0;

          if(buttonState[0])
            ocu.key_b=1;
          else
            ocu.key_b=0;  
          ocu.rc_att_w[0]=0;ocu.rc_att_w[1]=0;
          ocu.key_ll=ocu.key_rr=0;
        break;
        case M_TROT:
          if(connect_io){//连接IO板子
            ocu.rc_spd_w[0]=rc[0];
            ocu.rate_yaw_w=-rc[1];
            if(buttonState[2])
              ocu.rc_spd_w[1]=-1;
            else if(buttonState[1])
              ocu.rc_spd_w[1]=1;
            else 
              ocu.rc_spd_w[1]=0;
          }else{
                      
          if(WaytbuttonState[0])//up
            ocu.rc_spd_w[0]=0.8;
            else if(WaytbuttonState[1])//down
            ocu.rc_spd_w[0]=-0.8;
            else
            ocu.rc_spd_w[0]=0;
  
            if(WaytbuttonState[2])//left
              ocu.rc_spd_w[1]=-1;
            else if(WaytbuttonState[3])//right
              ocu.rc_spd_w[1]=1;
            else
              ocu.rc_spd_w[1]=0;
  
            if(buttonState[2])
              ocu.rate_yaw_w=0.7;
            else if(buttonState[1])
              ocu.rate_yaw_w=-0.7;
            else 
              ocu.rate_yaw_w=0;
          }

          if(buttonState[0])
            ocu.key_lr=1;
          else
            ocu.key_lr=0;

          ocu.rc_att_w[0]=0;ocu.rc_att_w[1]=0;
          ocu.key_ll=ocu.key_rr=0;
        break;
      }
    
      if(WaytbuttonState[4])//断电
        shut_down_cnt+=dt;
      else
        shut_down_cnt=0;
      
      if(shut_down_cnt>2){
        shut_down_cnt=0;
        ocu.key_ud=-1;
      }else if(ocu.key_ud==-1&&!WaytbuttonState[4])
        ocu.key_ud=0;
  }

  //
  for(int i=0;i<5;i++){
    WaytbuttonState_reg[i]=WaytbuttonState[i];
  }
  for(int i=0;i<3;i++){
    buttonState_reg[i]=buttonState[i];
  }
  delay(10);
}
